package Test;

import Behaviors.Arbitrators.*;
import Behaviors.Formations.*;
import Behaviors.Schemas.Motor.*;
import Behaviors.Schemas.Perception.*;

import Behaviors.Schemas.*;
import Robot.*;
import Robot.Sensors.Sonar;
import Utils.*;

import javaclient3.*;
import javaclient3.structures.*;


/**
 *
 * @author Alejandro Pustowka
 */
public class MaintainFormation {
    
    public static void main(String[] args){

        //numero de robots
        int numRobots = 3;

        //robot virtual.
        Robot robotLeader = new Robot("Virtual","localhost",6665, 5);
        //VirtualPosition2d positionLeader = new VirtualPosition2d(robotLeader,"p",5);

        Position2DInterface positionLeader = robotLeader.requestInterfacePosition2D(0, PlayerConstants.PLAYER_OPEN_MODE);

        Position2DHelper posHL = new Position2DHelper();

        //Parametros del control de rotacion y maxima velocidad lineal de los robots
        posHL.setRotControl(0.1f,0.3f);
        posHL.setMaxSpeed(0.2f);

        //robots reales.
        Robot robot1 = new Robot("Robot 1","localhost",6666, 1);
        Robot robot2 = new Robot("Robot 2","localhost",6667, 2);
        Robot robot3 = new Robot("Robot 3","localhost",6668, 3);
        Robot robot4 = new Robot("Robot 4","localhost",6669, 4);

        //interfaz de posicion de los robots reales.
        /*
        RealPosition2d position1=new RealPosition2d(robot1,"position1",0);
        RealPosition2d position2=new RealPosition2d(robot2,"position2",0);
        RealPosition2d position3=new RealPosition2d(robot3,"position3",0);
        RealPosition2d position4=new RealPosition2d(robot4,"position4",0);
        */

        Position2DInterface position1 = robot1.requestInterfacePosition2D(0, PlayerConstants.PLAYER_OPEN_MODE);
        Position2DInterface position2 = robot2.requestInterfacePosition2D(0, PlayerConstants.PLAYER_OPEN_MODE);
        Position2DInterface position3 = robot3.requestInterfacePosition2D(0, PlayerConstants.PLAYER_OPEN_MODE);
        Position2DInterface position4 = robot4.requestInterfacePosition2D(0, PlayerConstants.PLAYER_OPEN_MODE);

        //sonares de los robots reales.
        /*
        Sonar sonar1 = new Sonar(robot1,"sonar1",0);
        Sonar sonar2 = new Sonar(robot2,"sonar2",0);
        Sonar sonar3 = new Sonar(robot3,"sonar3",0);
        Sonar sonar4 = new Sonar(robot4,"sonar4",0);

         */
        SonarInterface sonar1 = robot1.requestInterfaceSonar(0, PlayerConstants.PLAYER_OPEN_MODE);
        SonarInterface sonar2 = robot2.requestInterfaceSonar(0, PlayerConstants.PLAYER_OPEN_MODE);
        SonarInterface sonar3 = robot3.requestInterfaceSonar(0, PlayerConstants.PLAYER_OPEN_MODE);
        SonarInterface sonar4 = robot4.requestInterfaceSonar(0, PlayerConstants.PLAYER_OPEN_MODE);

        //ejecucion del player en los robots.
        robot1.runThreaded(10,0);
        robot2.runThreaded(10,0);
        robot3.runThreaded(10,0);
        robot4.runThreaded(10,0);
        
        //actualizar valores de posiciones de ultrasonidos.
        /*
        while(!sonar1.updatePoses());
        while(!sonar2.updatePoses());
        while(!sonar3.updatePoses());
        while(!sonar4.updatePoses());

         */
        //instancias de las formaciones posibles.
        FormationSquare formQuad = new FormationSquare(numRobots);
        FormationArrow formArrow = new FormationArrow(numRobots);
        FormationRow formRow = new FormationRow (numRobots);
        FormationColumn formCol = new FormationColumn (numRobots);

        //array de formaciones y el generador de las mismas
        Formation[] array = {formQuad, formArrow, formRow, formCol};
        FormationGenerator formation=new FormationGenerator(array);
        formation.setWidth(4);
        formation.setCurrentFormation("ROW", 3, numRobots);

        //Posicion inicial de los robots.
        position1.setOdometry(new PlayerPose(-6.5f,6.5f,(float)Math.toRadians(-90.0f)));
        position2.setOdometry(new PlayerPose(-1.5f,7.5f,(float)Math.toRadians(-90.0f)));
        position3.setOdometry(new PlayerPose(2.5f,7.5f,(float)Math.toRadians(-135)));
        position4.setOdometry(new PlayerPose(2.5f,2.5f,(float)Math.toRadians(180)));

        Position2DHelper posH1 = new Position2DHelper();
        Position2DHelper posH2 = new Position2DHelper();
        Position2DHelper posH3 = new Position2DHelper();
        Position2DHelper posH4 = new Position2DHelper();

        //Parametros del control de rotacion de los robots
        posH1.setRotControl(0.5f,1.0f);
        posH2.setRotControl(0.5f,1.0f);
        posH3.setRotControl(0.5f,1.0f);
        posH4.setRotControl(0.5f,1.0f);

        //maxima velocidad lineal de los robots
        posH1.setMaxSpeed(0.6f);
        posH2.setMaxSpeed(0.6f);
        posH3.setMaxSpeed(0.6f);
        posH4.setMaxSpeed(0.6f);

        
        //la posicion inicial del robot virtual es el centroide de posicion de los otros robots
        float x = (float)Position2DHelper.getPosition(position1).getX();
        x += Position2DHelper.getPosition(position2).getX();
        x += Position2DHelper.getPosition(position3).getX();
        x += Position2DHelper.getPosition(position4).getX();

        x /= 4;

        float y = (float)Position2DHelper.getPosition(position1).getY();
        y += Position2DHelper.getPosition(position2).getY();
        y += Position2DHelper.getPosition(position3).getY();
        y += Position2DHelper.getPosition(position4).getY();

        y /= 4;

        //posicion inicial, maxima velocidad y control de rotacion del robot virtual.
        positionLeader.setOdometry(new PlayerPose(x,y,(float)Math.toRadians(-90.0f)));
        
        //punto meta
        Vector2d point=new Vector2d(10,0,Vector2d.MODE_XY);
        NxMSGoToPointNew goto2 = new NxMSGoToPointNew("goto2",positionLeader,point,4, 0.2f, NxMSGoToPoint.MODE_LINEAR);

        //comportamientos de los robots reales.
        NxPSDetectFormation detectFormation1 = new NxPSDetectFormation("goto2",formation,position1,positionLeader,0);
        NxPSDetectFormation detectFormation2 = new NxPSDetectFormation("goto2",formation,position2,positionLeader,1);
        NxPSDetectFormation detectFormation3 = new NxPSDetectFormation("goto2",formation,position3,positionLeader,2);
        NxPSDetectFormation detectFormation4 = new NxPSDetectFormation("goto2",formation,position4,positionLeader,3);

        NxMSGoToPointNew maintain1 = new NxMSGoToPointNew("goto2",detectFormation1,position1, 4, 0.2f, NxMSGoToPoint.MODE_EXPONENTIAL);
        NxMSGoToPointNew maintain2 = new NxMSGoToPointNew("goto2",detectFormation2,position2, 4, 0.2f, NxMSGoToPoint.MODE_EXPONENTIAL);
        NxMSGoToPointNew maintain3 = new NxMSGoToPointNew("goto2",detectFormation3,position3, 4, 0.2f, NxMSGoToPoint.MODE_EXPONENTIAL);
        NxMSGoToPointNew maintain4 = new NxMSGoToPointNew("goto2",detectFormation4,position4, 4, 0.2f, NxMSGoToPoint.MODE_EXPONENTIAL);
        
        NxPSDetectObstaclesSonarNew obstacles1 = new NxPSDetectObstaclesSonarNew("s1",position1,sonar1,true);
        NxPSDetectObstaclesSonarNew obstacles2 = new NxPSDetectObstaclesSonarNew("s2",position2,sonar2,true);
        NxPSDetectObstaclesSonarNew obstacles3 = new NxPSDetectObstaclesSonarNew("s3",position3,sonar3,true);
        NxPSDetectObstaclesSonarNew obstacles4 = new NxPSDetectObstaclesSonarNew("s4",position4,sonar4,true);

        obstacles1.setMinRange(0.2f);
        obstacles2.setMinRange(0.2f);
        obstacles3.setMinRange(0.2f);
        obstacles4.setMinRange(0.2f);

        
        NxPSMapObstaclesNew mobst1 = new NxPSMapObstaclesNew("m1",obstacles1,position1, 0.7f,-1, false);
        NxPSMapObstaclesNew mobst2 = new NxPSMapObstaclesNew("m2",obstacles2,position2, 0.7f,-1, false);
        NxPSMapObstaclesNew mobst3 = new NxPSMapObstaclesNew("m3",obstacles3,position3, 0.7f,-1, false);
        NxPSMapObstaclesNew mobst4 = new NxPSMapObstaclesNew("m4",obstacles4,position4, 0.7f,-1, false);

        NxMSAvoidObstaclesNew aobst1 = new NxMSAvoidObstaclesNew("a1",mobst1,position1,NxMSAvoidObstacles.MODE_EXPONENTIAL);
        NxMSAvoidObstaclesNew aobst2 = new NxMSAvoidObstaclesNew("a2",mobst2,position2,NxMSAvoidObstacles.MODE_EXPONENTIAL);
        NxMSAvoidObstaclesNew aobst3 = new NxMSAvoidObstaclesNew("a3",mobst3,position3,NxMSAvoidObstacles.MODE_EXPONENTIAL);
        NxMSAvoidObstaclesNew aobst4 = new NxMSAvoidObstaclesNew("a4",mobst4,position4,NxMSAvoidObstacles.MODE_EXPONENTIAL);

        NxMSAvoidObstaclesNew aobst11 = new NxMSAvoidObstaclesNew("a1",obstacles1,position1,NxMSAvoidObstacles.MODE_EXPONENTIAL);
        NxMSAvoidObstaclesNew aobst12 = new NxMSAvoidObstaclesNew("a2",obstacles2,position2,NxMSAvoidObstacles.MODE_EXPONENTIAL);
        NxMSAvoidObstaclesNew aobst13 = new NxMSAvoidObstaclesNew("a3",obstacles3,position3,NxMSAvoidObstacles.MODE_EXPONENTIAL);
        NxMSAvoidObstaclesNew aobst14 = new NxMSAvoidObstaclesNew("a4",obstacles4,position4,NxMSAvoidObstacles.MODE_EXPONENTIAL);

        NxMSNavigationVFHNew avoid1 = new NxMSNavigationVFHNew("avoidVFH",mobst1,maintain1,position1,NxMSAvoidObstacles.MODE_LINEAR,true);
        NxMSNavigationVFHNew avoid2 = new NxMSNavigationVFHNew("avoidVFH",mobst2,maintain2,position2,NxMSAvoidObstacles.MODE_LINEAR,true);
        NxMSNavigationVFHNew avoid3 = new NxMSNavigationVFHNew("avoidVFH",mobst3,maintain3,position3,NxMSAvoidObstacles.MODE_LINEAR,true);
        NxMSNavigationVFHNew avoid4 = new NxMSNavigationVFHNew("avoidVFH",mobst4,maintain4,position4,NxMSAvoidObstacles.MODE_LINEAR,true);

        NxMSNavigationVFHNew avoid11 = new NxMSNavigationVFHNew("avoidVFH",obstacles1,maintain1,position1,NxMSAvoidObstacles.MODE_LINEAR,false);
        NxMSNavigationVFHNew avoid12 = new NxMSNavigationVFHNew("avoidVFH",obstacles2,maintain2,position2,NxMSAvoidObstacles.MODE_LINEAR,false);
        NxMSNavigationVFHNew avoid13 = new NxMSNavigationVFHNew("avoidVFH",obstacles3,maintain3,position3,NxMSAvoidObstacles.MODE_LINEAR,false);
        NxMSNavigationVFHNew avoid14 = new NxMSNavigationVFHNew("avoidVFH",obstacles4,maintain4,position4,NxMSAvoidObstacles.MODE_LINEAR,false);



        aobst1.setExponentialDistance(0.3f);
        aobst2.setExponentialDistance(0.3f);
        aobst3.setExponentialDistance(0.3f);
        aobst4.setExponentialDistance(0.3f);
        aobst11.setExponentialDistance(0.3f);
        aobst12.setExponentialDistance(0.3f);
        aobst13.setExponentialDistance(0.3f);
        aobst14.setExponentialDistance(0.3f);

        avoid1.setExponentialDistance(0.3f);
        avoid2.setExponentialDistance(0.3f);
        avoid3.setExponentialDistance(0.3f);
        avoid4.setExponentialDistance(0.3f);

        avoid1.setValleyTresh(0.69f, 0.3f);
        avoid2.setValleyTresh(0.69f, 0.3f);
        avoid3.setValleyTresh(0.69f, 0.3f);
        avoid4.setValleyTresh(0.69f, 0.3f);


        aobst1.setGain(0.4f);
        aobst2.setGain(0.4f);
        aobst3.setGain(0.4f);
        aobst4.setGain(0.4f);
        aobst11.setGain(0.4f);
        aobst12.setGain(0.4f);
        aobst13.setGain(0.4f);
        aobst14.setGain(0.4f);

        maintain1.setGain(0.8f);
        maintain2.setGain(0.8f);
        maintain3.setGain(0.8f);
        maintain4.setGain(0.8f);

        //avoid1.setValleyTresh(1.5f, 0.7f);

        aobst11.setMaxRange(1);
        //PSChangeFormation chform1 = new PSChangeFormation (positionLeader, formation, obstaclesVirt, detectFormation1);

        //pila de comportamientos
        Behavior[] arrayLeader = {goto2};
        
        Behavior[] array1 = {avoid1,aobst1};//{maintain1,aobst11};
        Behavior[] array2 = {avoid2,aobst2};//{maintain2,aobst12};
        Behavior[] array3 = {avoid3,aobst3};//{maintain3,aobst13};
        Behavior[] array4 = {avoid4,aobst4};//{maintain4,aobst14};

        //arbitros
        CooperativeArbitratorNew arbitratorLeader = new CooperativeArbitratorNew("aL", arrayLeader, false);
        CooperativeArbitratorNew arbitrator1 = new CooperativeArbitratorNew("a1", array1, true);
        CooperativeArbitratorNew arbitrator2 = new CooperativeArbitratorNew("a2", array2, false);
        CooperativeArbitratorNew arbitrator3 = new CooperativeArbitratorNew("a3", array3, false);
        CooperativeArbitratorNew arbitrator4 = new CooperativeArbitratorNew("a4", array4, false);

        boolean enable = true;

        arbitrator1.enabled = enable;
        arbitrator2.enabled = enable;
        arbitrator3.enabled = enable;
        arbitrator4.enabled = enable;


        //Interfaz para cambiar de formaciones, el ancho y el punto meta del robot
        //virtual
        JFrameChangeForm frame = new JFrameChangeForm(formation,point,positionLeader);
        frame.setVisible(true);

        boolean firstTime = false;

        //TODO: Probar sin estos while!!!
        while(!position1.isDataReady());
        while(!position2.isDataReady());
        while(!position3.isDataReady());
        while(!position4.isDataReady());

        while(!sonar1.isDataReady());
        while(!sonar2.isDataReady());
        while(!sonar3.isDataReady());
        while(!sonar4.isDataReady());

        while(true){

            Vector2d totalL;

            float dist1 = (float)maintain1.Value().getMag();
            float dist2 = (float)maintain1.Value().getMag();
            float dist3 = (float)maintain1.Value().getMag();
            float dist4 = (float)maintain1.Value().getMag();

            boolean pos1 =  dist1 < 0.2f;
            boolean pos2 =  dist2 < 0.2f;
            boolean pos3 =  dist3 < 0.2f;
            boolean pos4 =  dist4 < 0.2f;

            if(pos1 && pos2 && pos3 && pos4 || firstTime)
            {
                totalL = arbitratorLeader.Value();
                firstTime = true;
            }
            else
            {
                //el robot virtual no avanza
                //TODO: Mejorar esto. Definir la velocidad del robot virtual
                // a partir de la distancia de cada robot a su posicion.
                //Es decir, si los robots estan cerca, entonces el robot virtual
                // solo aminora su marcha. Si los robots estan lejos, el robot
                //virtual para.
                positionLeader.setSpeed(0,0);


                if(false)
                {
                    //se recalcula la posicion del robot virtual
                    x = (float)Position2DHelper.getPosition(position1).getX();
                    x += Position2DHelper.getPosition(position2).getX();
                    x += Position2DHelper.getPosition(position3).getX();
                    x += Position2DHelper.getPosition(position4).getX();

                    x /= 4;

                    y = (float)Position2DHelper.getPosition(position1).getY();
                    y += Position2DHelper.getPosition(position1).getY();
                    y += Position2DHelper.getPosition(position1).getY();
                    y += Position2DHelper.getPosition(position1).getY();

                    y /= 4;

                    Vector2d posL = Position2DHelper.getPosition(positionLeader);

                    y += posL.getY();
                    y /= 2;

                    x += posL.getX();
                    x /= 2;


                    positionLeader.setOdometry(new PlayerPose(x,y,(float)Math.toRadians(-90.0f)));
                }
            }
            Vector2d total1 = arbitrator1.Value();
            Vector2d total2 = arbitrator2.Value();
            Vector2d total3 = arbitrator3.Value();
            Vector2d total4 = arbitrator4.Value();

            PlayerPose total11 = total1.toPlayerPose();
            PlayerPose total22 = total2.toPlayerPose();
            PlayerPose total33 = total3.toPlayerPose();
            PlayerPose total44 = total4.toPlayerPose();


            posH1.goTo(total11, position1);
            posH2.goTo(total22, position2);
            posH3.goTo(total33, position3);
            posH4.goTo(total44, position4);


            //Pose2d posL = positionLeader.read(Position2d.READ_MODE_NO_WAIT);

            //posL.print();

            
            try
            {
                Thread.sleep(20);
            }
            catch(Exception e)
            {
            }

        }
            
    }
}